#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Integrated Vehicle Controller with Lane Following and Traffic Sign Actions

Copyright (c) 2025 Zachary and HoSheil
All rights reserved.

This code is exclusively for competition customers use only!
Unauthorized reproduction, distribution, or modification is prohibited.

Author: Zachary and HoSheil
Date: 2025
Version: 2.0

Description:
    This ROS node integrates yellow lane detection/following with traffic sign action execution.
    Key features include:
    - Yellow lane detection using HSV color filtering and Hough line detection
    - Adaptive steering control based on detected lane lines
    - Traffic sign detection response (left turn, reverse parking, parallel parking)
    - Support for single-side lane detection with configurable offset
    - Real-time lane following enable/disable control
    - Interactive debugging with keyboard controls
    - Safety mechanisms and emergency stop functionality
    
    Controls:
    - Space: Toggle lane following on/off
    - '1': Execute left turn
    - '2': Execute reverse parking
    - '3': Execute parallel parking
    - 'h': Show help information
    - 'o'/'p': Adjust lane offset
    - 'c': Toggle forward/reverse mode
    - 'r': Reset parking status
    - 's': Show current status
    - 'q': Quit safely
"""

import rospy
import cv2
import numpy as np
import json
import math
import time
import threading
import sys
import select
import tty
import termios
import traceback
from sensor_msgs.msg import Image
from abot_driver.msg import Akm
from nav_msgs.msg import Odometry
from std_msgs.msg import String
from cv_bridge import CvBridge, CvBridgeError
from tf.transformations import euler_from_quaternion

class KeyboardInput:
    """键盘输入处理类"""
    def __init__(self):
        self.old_settings = None
        
    def __enter__(self):
        try:
            self.old_settings = termios.tcgetattr(sys.stdin)
            tty.cbreak(sys.stdin.fileno())
        except AttributeError:
            try:
                self.old_settings = termios.tcgetattr(sys.stdin)
                tty.setcbreak(sys.stdin.fileno())
            except AttributeError:
                self.old_settings = termios.tcgetattr(sys.stdin)
                tty.setraw(sys.stdin.fileno())
        return self
        
    def __exit__(self, type, value, traceback):
        if self.old_settings:
            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)
        
    def get_char(self):
        """获取单个字符输入"""
        try:
            if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []):
                return sys.stdin.read(1)
        except Exception:
            pass
        return None

class IntegratedVehicleController:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('integrated_vehicle_controller', anonymous=True)
        
        # === ROS参数配置 ===
        # 话题配置
        self.camera_topic = rospy.get_param('~camera_topic', '/camera/rgb/image_raw')
        self.back_camera_topic = '/camera/back/image_raw'
        self.detection_topic = rospy.get_param('~detection_topic', '/yolo_detections')
        self.cmd_topic = rospy.get_param('~cmd_topic', '/akm_vel')
        self.odom_topic = rospy.get_param('~odom_topic', '/odom')
        self.debug = rospy.get_param('~debug', True)
        
        # === 车道线跟随参数 ===
        # 前进模式参数
        self.forward_params = {
            'speed': rospy.get_param('~forward_speed', 0.3),
            'lane_offset': rospy.get_param('~forward_lane_offset', 180),  
            'history_length': rospy.get_param('~forward_history_length', 5),  # 增加历史长度，增强直行惯性
            'steering_smoothing_factor': rospy.get_param('~forward_steering_smoothing', 0.30),  # 大幅增加平滑因子
            'adaptive_max_change': rospy.get_param('~forward_adaptive_max_change', 0.12),  # 增加最大变化量 0.08
            'base_steering_gain': rospy.get_param('~forward_base_steering_gain', 1.5),  # 基础转向增益
            'curve_detection_threshold': rospy.get_param('~forward_curve_threshold', 0.04),  # 降低弯道检测阈值
            'prediction_weight': rospy.get_param('~forward_prediction_weight', 0.5),  # 增加预测权重
            'max_change': rospy.get_param('~forward_max_change', 0.18),  # 最大转向变化限制
            'max_steering': rospy.get_param('~forward_max_steering', 0.5),  # 最大steering值 
            'steering_coefficient': rospy.get_param('~forward_steering_coeff', 2.4),  # 转向系数 
            'left_contour_threshold': 20,  # 降低阈值，增强左线检测
            'right_contour_threshold': 20,  # 降低阈值，增强右线检测
            'arc_length_threshold': 15  # 降低弧长阈值
        }
        
        # 倒车模式参数 - 增强转弯幅度
        self.reverse_params = {
            'speed': rospy.get_param('~reverse_speed', 0.2),
            'lane_offset': rospy.get_param('~reverse_lane_offset', 450),  # 增加倒车偏移量
            'history_length': rospy.get_param('~reverse_history_length', 6),  # 减少历史长度
            'steering_smoothing_factor': rospy.get_param('~reverse_steering_smoothing', 0.2),  # 减少平滑因子
            'adaptive_max_change': rospy.get_param('~reverse_adaptive_max_change', 0.15),  # 增加变化量
            'base_steering_gain': rospy.get_param('~reverse_base_steering_gain', 1.8),  # 增加倒车转向增益
            'curve_detection_threshold': rospy.get_param('~reverse_curve_threshold', 0.04),  # 降低弯道检测阈值
            'prediction_weight': rospy.get_param('~reverse_prediction_weight', 0.25),  # 增加预测权重
            'max_change': rospy.get_param('~reverse_max_change', 0.16),  # 增加最大变化
            'steering_coefficient': rospy.get_param('~reverse_steering_coeff', 1.0),  # 显著增加倒车转向系数
            'left_contour_threshold': 30,
            'right_contour_threshold': 30,
            'arc_length_threshold': 10
        }
        
        # S弯模式参数 - 增强转向惯性，防止过早切换方向
        self.s_curve_params = {
            'speed': rospy.get_param('~s_curve_speed', 0.3),  # 保持正常速度
            'lane_offset': rospy.get_param('~s_curve_lane_offset', 270),  # 稍大的偏移
            'history_length': rospy.get_param('~s_curve_history_length', 7),  # 增大历史长度，增强转向惯性
            'steering_smoothing_factor': rospy.get_param('~s_curve_steering_smoothing', 0.32),  # 大幅增加平滑因子，让转向变化缓慢
            'adaptive_max_change': rospy.get_param('~s_curve_adaptive_max_change', 0.3),
            'base_steering_gain': rospy.get_param('~s_curve_base_steering_gain', 1.8),
            'curve_detection_threshold': rospy.get_param('~s_curve_curve_threshold', 0.04),
            'prediction_weight': rospy.get_param('~s_curve_prediction_weight', 0.6),  # 增加预测权重
            'max_change': rospy.get_param('~s_curve_max_change', 0.18),  # 减小最大变化率，防止突然改变方向
            'max_steering': rospy.get_param('~s_curve_max_steering', 0.5),
            'steering_coefficient': rospy.get_param('~s_curve_steering_coeff', 2.0),
            'left_contour_threshold': 15,
            'right_contour_threshold': 18,
            'arc_length_threshold': 12
        }
        
        # === 交通标志动作参数 - 增强转弯能力 ===
        self.max_linear_speed = rospy.get_param('~max_linear_speed', 1.2)  # 增加最大线速度
        self.max_angular_speed = rospy.get_param('~max_angular_speed', 2)  # 显著增加最大角速度
        self.confidence_threshold = rospy.get_param('~confidence_threshold', 0.7)
        self.turn_linear_speed = rospy.get_param('~turn_linear_speed', 0.25)
        self.turn_angular_speed = rospy.get_param('~turn_angular_speed', 1.2)  # 增加转弯角速度
        self.parking_linear_speed = rospy.get_param('~parking_linear_speed', 0.12)
        self.parking_angular_speed = rospy.get_param('~parking_angular_speed', 0.8)  # 增加停车动作的角速度
        self.detection_cooldown = rospy.get_param('~detection_cooldown', 5.0)
        
        # === 状态变量 ===
        # 车道跟随状态
        self.slow_mode = False #单边桥慢速
        self.lane_following_enabled = False  # 初始不启动巡线
        self.reverse_mode = False
        self.s_curve_mode = False  # S弯模式标志（第三次左转后激活）
        self.current_camera_topic = self.camera_topic
        
        # 动作执行状态
        self.is_executing_action = False
        self.reverse_parking_executed = False
        self.parallel_parking_executed = False
        self.allow_action_reset = rospy.get_param('~allow_action_reset', True)
        
        # 新增动作执行状态
        self.left_turn_executed = False
        self.yaxian_executed = False
        self.daoche_executed = False
        self.fanzhuan_executed = False
        self.final_executed = False
        
        # S弯控制相关
        self.first_turn_completed_time = None  # 第一次左转完成时间
        
        # 位姿和检测状态
        self.current_pose = None
        self.current_yaw = 0.0
        self.last_detection_time = 0
        self.last_executed_action = None
        self.last_action_time = 0
        
        # 动作执行计数器
        self.action_counts = {
            'turn_right': 0,
            'reverse': 0,
            'parallel_parking': 0,
            'left_turn': 0,
            'yaxian': 0,
            'stop': 0,
            'daoche': 0,
            'fanzhuan': 0,
            'final': 0
        }
        
        # 左转次数限制
        self.max_right_turns = 3
        
        # === 车道线检测变量 ===
        self.prev_steering = 0.0
        self.lane_history = []
        self.steering_history = []
        self.no_lane_count = 0
        self.last_steering_time = time.time()
        
        # 添加车道线历史跟踪 - 调整为支持更大转弯
        self.left_lane_history = []  # 左侧车道线历史位置
        self.right_lane_history = []  # 右侧车道线历史位置
        self.lane_consistency_threshold = 120  # 增加车道线一致性阈值以适应大转弯
        self.max_lane_history = 2  # 减少最大历史记录数以提高响应性
        
        # 黄色车道线HSV范围
        self.yellow_lower = np.array([15, 80, 100])
        self.yellow_upper = np.array([40, 255, 255])
        
        # === 线程和显示相关 ===
        self.display_lock = threading.Lock()
        self.result_img = None
        self.roi_img = None
        self.mask_img = None
        
        # 键盘控制
        self.keyboard_enabled = rospy.get_param('~keyboard_enabled', True)
        self.keyboard_thread = None
        self.running = True
        
        # === S弯模式变量 ===
        self.s_curve_activation_time = None  # S弯模式激活时间
        self.s_curve_duration = 2.5  # S弯模式持续时间（秒）
        rospy.loginfo("S弯模式参数已初始化")
        
        # === ROS组件初始化 ===
        self.bridge = CvBridge()
        self.cmd_pub = rospy.Publisher(self.cmd_topic, Akm, queue_size=10)
        self.image_sub = rospy.Subscriber(self.current_camera_topic, Image, self.image_callback)
        self.detection_sub = rospy.Subscriber(self.detection_topic, String, self.detection_callback)
        self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry, self.odom_callback)
        
        # 注册关闭处理函数
        rospy.on_shutdown(self.cleanup)
        
        # 调试窗口
        if self.debug:
            cv2.namedWindow('Lane Detection', cv2.WINDOW_NORMAL)
            cv2.resizeWindow('Lane Detection', 640, 480)
        
        # 启动键盘控制
        if self.keyboard_enabled:
            self.start_keyboard_thread()
            self.print_help()
        
        rospy.loginfo("集成车辆控制器已启动")
        rospy.loginfo(f"前进参数: {self.forward_params}")
        rospy.loginfo(f"倒车参数: {self.reverse_params}")
        rospy.loginfo(f"车道跟随状态: {'开启' if self.lane_following_enabled else '关闭'}")
        rospy.loginfo(f"检测冷却时间: {self.detection_cooldown}秒")

    # === 基础功能方法 ===
    def get_current_params(self):
        # 倒车模式优先（最高优先级）
        if self.reverse_mode:
            rospy.logdebug("使用倒车参数")
            return self.reverse_params
        
        # S弯模式（仅在前进模式下生效）
        elif self.s_curve_mode:
            rospy.logdebug("使用S弯参数")
            return self.s_curve_params
        
        # 默认前进模式
        else:
            rospy.logdebug("使用前进参数")
            return self.forward_params
        
    def get_param(self, param_name):
        """获取当前模式下的特定参数"""
        current_params = self.get_current_params()
        return current_params.get(param_name, 0)

    def cleanup(self):
        """清理函数，在节点关闭时调用"""
        rospy.loginfo("正在关闭集成车辆控制器...")
        self.running = False
        self.stop_vehicle()     
        cv2.destroyAllWindows()
        rospy.loginfo("节点已安全关闭")
    
    def stop_vehicle(self):
        """发布停止指令"""
        stop_akm = Akm()
        stop_akm.linear = 0.0
        stop_akm.angular = 0.0
        stop_akm.diff = 0.0
        self.cmd_pub.publish(stop_akm)

    def publish_cmd_vel(self, linear_x, angular_z):
        """发布akm_vel控制命令"""
        cmd = Akm()
        cmd.linear = linear_x
        cmd.angular = angular_z
        cmd.diff = 0.0  # 默认差速为0
        self.cmd_pub.publish(cmd)

    # === 键盘控制相关 ===
    def start_keyboard_thread(self):
        """启动键盘监听线程"""
        self.keyboard_thread = threading.Thread(target=self.keyboard_listener)
        self.keyboard_thread.daemon = True
        self.keyboard_thread.start()
        rospy.loginfo("键盘控制已启用")
        
    def keyboard_listener(self):
        """键盘监听线程函数"""
        try:
            with KeyboardInput() as kb:
                while self.running and not rospy.is_shutdown():
                    char = kb.get_char()
                    if char:
                        self.handle_keyboard_input(char)
                    time.sleep(0.1)
        except Exception as e:
            rospy.logerr(f"键盘监听错误: {e}")
            
    def handle_keyboard_input(self, char):
        """处理键盘输入"""
        if char == '1':
            rospy.loginfo("键盘触发：执行左转动作")
            if not self.is_executing_action and self.action_counts['turn_right'] < self.max_right_turns:
                threading.Thread(target=self.execute_left_turn).start()
            else:
                rospy.logwarn("正在执行其他动作或左转次数已达上限，请稍后再试")
                
        elif char == '2':
            rospy.loginfo("键盘触发：执行倒车入库动作")
            if not self.is_executing_action:
                threading.Thread(target=self.execute_reverse_parking).start()
            else:
                rospy.logwarn("正在执行其他动作，请稍后再试")
                
        elif char == '3':
            rospy.loginfo("键盘触发：执行侧方停车动作")
            if not self.is_executing_action:
                threading.Thread(target=self.execute_parallel_parking).start()
            else:
                rospy.logwarn("正在执行其他动作，请稍后再试")
                
        elif char == '4':
            rospy.loginfo("键盘触发：执行右转动作")
            if not self.is_executing_action and not self.left_turn_executed:
                threading.Thread(target=self.execute_left_turn).start()
            else:
                rospy.logwarn("正在执行其他动作或右转已执行过，请稍后再试")
                
        elif char == '5':
            rospy.loginfo("键盘触发：执行靠左行驶动作")
            if not self.is_executing_action and not self.yaxian_executed:
                threading.Thread(target=self.execute_move_left_and_start_lane_following).start()
            else:
                rospy.logwarn("正在执行其他动作或靠左行驶已执行过，请稍后再试")
                
        elif char == '6':
            rospy.loginfo("键盘触发：执行停车动作")
            if not self.is_executing_action:
                threading.Thread(target=self.execute_stop_and_continue).start()
            else:
                rospy.logwarn("正在执行其他动作，请稍后再试")
                
        elif char == '7':
            rospy.loginfo("键盘触发：执行倒车巡线动作")
            if not self.is_executing_action and not self.daoche_executed:
                threading.Thread(target=self.execute_reverse_lane_following).start()
            else:
                rospy.logwarn("正在执行其他动作或倒车巡线已执行过，请稍后再试")
                
        elif char == '8':
            rospy.loginfo("键盘触发：执行左后方倒车动作")
            if not self.is_executing_action and not self.fanzhuan_executed:
                threading.Thread(target=self.execute_left_rear_reverse).start()
            else:
                rospy.logwarn("正在执行其他动作或左后方倒车已执行过，请稍后再试")
                
        elif char == '9':
            rospy.loginfo("键盘触发：执行最终停止动作")
            if not self.is_executing_action and not self.final_executed:
                threading.Thread(target=self.execute_final_stop).start()
            else:
                rospy.logwarn("正在执行其他动作或最终停止已执行过，请稍后再试")
                
        elif char == 'r' or char == 'R':
            rospy.loginfo("键盘触发：重置停车状态")
            self.reset_parking_status()
            # 同时清除车道线历史
            self.left_lane_history.clear()
            self.right_lane_history.clear()
            rospy.loginfo("已清除车道线历史数据")
            
        elif char == 's' or char == 'S':
            rospy.loginfo("键盘触发：显示当前状态")
            self.print_current_status()
            
        elif char == 'h' or char == 'H':
            self.print_help()
            
        elif char == 'q' or char == 'Q':
            rospy.loginfo("键盘触发：退出程序")
            self.shutdown()
            
        elif char == ' ':
            self.toggle_lane_following()
            
        elif char == 'c' or char == 'C':
            self.toggle_reverse_mode()
            
        elif char == 'o':
            current_params = self.get_current_params()
            current_params['lane_offset'] += 10
            rospy.loginfo(f"增加车道偏移到 {current_params['lane_offset']}px")
            
        elif char == 'p':
            current_params = self.get_current_params()
            current_params['lane_offset'] = max(50, current_params['lane_offset'] - 10)
            rospy.loginfo(f"减少车道偏移到 {current_params['lane_offset']}px")

    def print_help(self):
        """显示帮助信息"""
        help_text = """
        ========== 集成车辆控制器帮助 ==========
        车道跟随控制:
        空格键: 开启/关闭巡线功能
        'c': 切换前进/倒车模式
        'o': 增加车道偏移 (+10px)
        'p': 减少车道偏移 (-10px)
        
        交通标志动作:
        '1': 执行左转动作 (最多3次)
        '2': 执行倒车入库动作
        '3': 执行侧方停车动作
        '4': 执行右转动作
        '5': 执行靠左行驶动作
        '6': 执行停车动作
        '7': 执行倒车巡线动作
        '8': 执行左后方倒车动作
        '9': 执行最终停止动作
        'r': 重置停车状态和车道线历史
        
        系统控制:
        'h': 显示此帮助信息
        's': 显示当前状态
        'q': 安全退出程序
        
        当前状态:
        - 巡线功能: {}
        - 当前模式: {}
        - 摄像头: {}
        - 速度: {:.2f} m/s
        - 车道偏移: {} px
        - 左转次数: {}/{}
        - 左侧历史: {} 条 | 右侧历史: {} 条
        - 一致性阈值: {} px
        =====================================
        """.format(
            "开启" if self.lane_following_enabled else "关闭",
            "倒车" if self.reverse_mode else "前进",
            self.current_camera_topic.split('/')[-2],
            self.get_param('speed'),
            self.get_param('lane_offset'),
            self.action_counts['turn_right'],
            self.max_right_turns,
            len(self.left_lane_history),
            len(self.right_lane_history),
            self.lane_consistency_threshold
        )
        
        rospy.loginfo(help_text)

    def print_current_status(self):
        """打印当前状态信息"""
        print("\n=== 当前状态 ===")
        print(f"车道跟随: {'开启' if self.lane_following_enabled else '关闭'}")
        
        # 显示当前控制模式
        if self.s_curve_mode:
            print(f"当前模式: S弯模式 (惯性增强)")
        elif self.reverse_mode:
            print(f"当前模式: 倒车")
        else:
            print(f"当前模式: 前进")
        print(f"正在执行动作: {self.is_executing_action}")
        print(f"倒车入库已执行: {self.reverse_parking_executed}")
        print(f"侧方停车已执行: {self.parallel_parking_executed}")
        print(f"动作执行计数: {self.action_counts}")
        print(f"最后执行动作: {self.last_executed_action}")
        print(f"S弯模式: {'激活 ' if self.s_curve_mode else '未激活'}")
        
        # 显示S弯时间信息
        if self.first_turn_completed_time is not None:
            elapsed = time.time() - self.first_turn_completed_time
            if elapsed < 1.0:
                print(f" 普通模式调整中（{elapsed:.1f}s / 1.0s）")
            elif self.s_curve_mode and self.s_curve_activation_time:
                s_elapsed = time.time() - self.s_curve_activation_time
                remaining = self.s_curve_duration - s_elapsed
                print(f" S弯模式运行中（剩余 {remaining:.1f}s / {self.s_curve_duration}s）")
        
        if self.current_pose:
            print(f"当前位置: x={self.current_pose.position.x:.2f}, y={self.current_pose.position.y:.2f}")
            print(f"当前角度: {math.degrees(self.current_yaw):.1f}度")
        else:
            print("位置信息: 不可用")
        print("===============\n")
        
    def shutdown(self):
        """关闭程序"""
        self.running = False
        self.stop_vehicle()
        rospy.signal_shutdown("用户请求退出")

    # === 回调函数 ===
    def odom_callback(self, msg):
        """里程计回调函数，更新当前位姿"""
        self.current_pose = msg.pose.pose
        orientation = msg.pose.pose.orientation
        _, _, self.current_yaw = euler_from_quaternion([
            orientation.x, orientation.y, orientation.z, orientation.w
        ])

    def detection_callback(self, msg):
        """YOLO检测结果回调函数"""
        if self.is_executing_action:
            rospy.logdebug("正在执行动作，跳过此次检测")
            return
            
        current_time = rospy.get_time()
        if current_time - self.last_detection_time < self.detection_cooldown:
            rospy.logdebug(f"检测冷却中，剩余时间: {self.detection_cooldown - (current_time - self.last_detection_time):.1f}秒")
            return
            
        try:
            detection_data = json.loads(msg.data)
            detections = detection_data.get('detections', [])
            
            # 检查是否同时检测到greenlight和turn_right
            greenlight_detected = False
            turn_left_detected = False
            
            # 寻找置信度最高的检测结果
            best_detection = None
            max_confidence = 0
            
            for detection in detections:
                confidence = detection.get('confidence', 0)
                class_name = detection.get('class_name', '')
                
                if confidence > self.confidence_threshold:
                    if class_name == 'green':
                        greenlight_detected = True
                    elif class_name == 'turn_left':
                        turn_left_detected = True
                    
                    if confidence > max_confidence:
                        max_confidence = confidence
                        best_detection = detection
            
            # 处理greenlight + turn_right的组合检测
            if turn_left_detected and not self.left_turn_executed:
            #if greenlight_detected and turn_left_detected and not self.left_turn_executed:
                rospy.loginfo("检测到绿灯和左转标志，执行左转动作")
                self.lane_following_enabled = False
                threading.Thread(target=self.execute_left_turn).start()
                self.last_detection_time = current_time
                return
            
            if best_detection:
                class_name = best_detection['class_name']
                rospy.loginfo(f"检测到: {class_name}, 置信度: {max_confidence:.3f}")
                
                # 检查是否与上次执行的动作相同且时间间隔过短
                if (self.last_executed_action == class_name and 
                    current_time - self.last_action_time < self.detection_cooldown):
                    rospy.logwarn(f"动作 {class_name} 刚刚执行过，跳过重复执行")
                    return
                
                # 根据检测结果执行相应动作
                action_executed = False
                
                # 【逻辑2】检测到turn_right时，如果右转未执行，执行右转
                if class_name == 'turn_right':
                    if not self.left_turn_executed:
                        rospy.loginfo(" 检测到右转标志，但左转未执行 → 执行左转")
                        self.lane_following_enabled = False
                        threading.Thread(target=self.execute_right_turn).start()
                        action_executed = True
                    elif self.action_counts['turn_right'] < self.max_right_turns:
                        self.lane_following_enabled = False
                        threading.Thread(target=self.execute_right_turn).start()
                        action_executed = True
                    else:
                        rospy.logwarn("左转次数已达上限，跳过此次检测")
                        
                # 【逻辑3、4】检测到reverse
                elif class_name == 'reverse':
                    if self.daoche_executed and self.fanzhuan_executed:
                        self.lane_following_enabled = False
                        threading.Thread(target=self.execute_reverse_parking).start()
                        action_executed = True
                    ## 2.  关键：reverse 已执行，但 daoche 没执行过 → 直接进入倒车巡线
                    elif not self.daoche_executed and self.action_counts['turn_right'] == 1:
                        rospy.loginfo("daoche 未做 → 直接执行倒车巡线")
                        self.lane_following_enabled = False
                        threading.Thread(target=self.execute_reverse_lane_following).start()
                        action_executed = True
                    else:
                        rospy.logwarn("倒车入库动作已执行过，跳过此次检测")
                        
                elif class_name == 'Parallel_parking':
                    if not self.parallel_parking_executed:
                        self.lane_following_enabled = False
                        threading.Thread(target=self.execute_parallel_parking).start()
                        action_executed = True
                    else:
                        rospy.logwarn("侧方停车动作已执行过，跳过此次检测")
                
                elif class_name == 'yaxian':
                    if not self.yaxian_executed:
                        self.lane_following_enabled = False
                        threading.Thread(target=self.execute_move_left_and_start_lane_following).start()
                        action_executed = True
                        self.slow_mode = False
                    else:
                        rospy.logwarn("靠左行驶动作已执行过，跳过此次检测")
                
                elif class_name == 'stop':
                    if self.fanzhuan_executed:
                        threading.Thread(target=self.execute_stop_and_continue).start()
                        action_executed = True
                    else:
                        rospy.logwarn("未执行倒车入库，跳过停车动作")
                
                # 【逻辑3、5】检测到daoche
                elif class_name == 'daoche':
                    # 逻辑3：如果yaxian未执行，先执行压线
                    if not self.yaxian_executed:
                        rospy.loginfo(" 检测到倒车巡线标志，但压线未执行 → 执行压线")
                        self.lane_following_enabled = False
                        threading.Thread(target=self.execute_move_left_and_start_lane_following).start()
                        action_executed = True
                    # 正常：执行倒车巡线
                    elif (self.action_counts['turn_right'] == 1 and not self.daoche_executed):
                        self.lane_following_enabled = False
                        threading.Thread(target=self.execute_reverse_lane_following).start()
                        action_executed = True
                    else:
                        rospy.logwarn("左转次数不足3次或倒车巡线已执行过，跳过此次检测")
                
                elif class_name == 'fanzhuan':
                    if (self.daoche_executed and not self.fanzhuan_executed):
                        self.lane_following_enabled = False
                        threading.Thread(target=self.execute_left_rear_reverse).start()
                        action_executed = True
                    else:
                        rospy.logwarn("倒车巡线未执行或左后方倒车已执行过，跳过此次检测")
                
                elif class_name == 'final':
                    if (self.fanzhuan_executed and not self.final_executed):
                        threading.Thread(target=self.execute_final_stop).start()
                        action_executed = True
                    else:
                        rospy.logwarn("左后方倒车未执行或最终停止已执行过，跳过此次检测")
                
                # 更新执行记录
                if action_executed:
                    self.last_detection_time = current_time
                    self.last_executed_action = class_name
                    self.last_action_time = current_time
                    self.action_counts[class_name] += 1
                    rospy.loginfo(f"动作执行计数: {self.action_counts}")
                
        except json.JSONDecodeError as e:
            rospy.logerr(f"JSON解析错误: {e}")
        except Exception as e:
            rospy.logerr(f"检测处理错误: {e}")

    def image_callback(self, data):
        """图像回调函数 - 支持慢速循迹模式"""
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            rospy.logerr(e)
            return
        
        steering, result_img = self.detect_yellow_lanes(cv_image)
        
        
        # 优先级1: 正在执行动作时，不发送任何控制指令
        if self.is_executing_action:
            pass  # 动作执行期间，不干预控制
        
        # 优先级2: 巡线模式开启（正常或慢速）
        elif self.lane_following_enabled:
            if self.parallel_parking_executed and self.action_counts['turn_right'] == 1:
                # 慢速直行，不跟随转向
                self.publish_cmd_vel(0.2, 0.0)
                rospy.logdebug(" 慢速巡线模式（侧方停车后）")
            elif not self.parallel_parking_executed and self.reverse_parking_executed and self.action_counts['turn_right'] == 1:
                self.publish_cmd_vel(0.2, 0.0)
            else:
                # 正常巡线
                self.publish_steering(steering)
        
        # 优先级3: 慢速直行模式（巡线关闭，但慢速开启）
        elif self.slow_mode:
            # 仅直行，不转向（适合过渡阶段）
            self.publish_cmd_vel(0.12, 0)  # 可调整的慢速
            rospy.logdebug(" 慢速直行模式")
        
        # 优先级4: 完全停止
        else:
            # 既没开巡线，也没开慢速，保持停止
            pass
        
        with self.display_lock:
            self.result_img = result_img


    # === 车道线检测相关方法 ===
    def enhanced_lane_detection(self, yellow_mask):
        """增强的车道线检测方法 - 解决左右线混淆问题"""
        height, width = yellow_mask.shape[:2]
        
        # 形态学操作
        current_params = self.get_current_params()
        kernel_size = 3 if self.reverse_mode else 5
        kernel = np.ones((kernel_size, kernel_size), np.uint8)
        
        yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_CLOSE, kernel)
        yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_OPEN, kernel)
        
        # 检测所有轮廓
        contours, _ = cv2.findContours(yellow_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        
        # 过滤和预处理轮廓
        left_threshold = current_params.get('left_contour_threshold', 50)
        right_threshold = current_params.get('right_contour_threshold', 100)
        arc_threshold = current_params.get('arc_length_threshold', 30)
        
        valid_lines = []
        
        for contour in contours:
            area = cv2.contourArea(contour)
            arc_length = cv2.arcLength(contour, False)
            
            # 基本过滤
            if area > min(left_threshold, right_threshold) and arc_length > arc_threshold:
                points = contour.reshape(-1, 2)
                
                # 计算线条的基本几何特征
                line_features = self.calculate_line_features(points, width, height)
                
                if line_features is not None:
                    valid_lines.append({
                        'points': points,
                        'features': line_features
                    })
        
        # 使用改进的分类算法分离左右线
        left_lines, right_lines = self.classify_lane_lines(valid_lines, width, height)
        
        # 添加历史一致性检查
        left_lines = self.validate_with_history(left_lines, 'left', width)
        right_lines = self.validate_with_history(right_lines, 'right', width)
        
        return left_lines, right_lines
    
    def calculate_line_features(self, points, img_width, img_height):
        """计算线条的几何特征"""
        if len(points) < 3:
            return None
            
        # 计算中心点和边界
        center_x = np.mean(points[:, 0])
        center_y = np.mean(points[:, 1])
        
        min_x, max_x = np.min(points[:, 0]), np.max(points[:, 0])
        min_y, max_y = np.min(points[:, 1]), np.max(points[:, 1])
        
        # 计算线条角度（使用线性回归）
        if len(points) >= 3:
            try:
                # 拟合直线
                vx, vy, x0, y0 = cv2.fitLine(points, cv2.DIST_L2, 0, 0.01, 0.01)
                angle = np.arctan2(vy[0], vx[0]) * 180 / np.pi
                
                # 计算线条长度
                line_length = np.sqrt((max_x - min_x)**2 + (max_y - min_y)**2)
                
                # 计算相对位置（相对于图像中心）
                relative_x = center_x / img_width
                
                return {
                    'center_x': center_x,
                    'center_y': center_y,
                    'relative_x': relative_x,
                    'angle': angle,
                    'length': line_length,
                    'width_span': max_x - min_x,
                    'height_span': max_y - min_y,
                    'min_x': min_x,
                    'max_x': max_x,
                    'min_y': min_y,
                    'max_y': max_y
                }
            except:
                return None
        
        return None
    
    def classify_lane_lines(self, valid_lines, img_width, img_height):
        """基于几何特征和约束的车道线分类（参考src.py稳定版）"""
        if not valid_lines:
            return [], []
        
        img_center = img_width / 2
        left_lines = []
        right_lines = []
        
        # 预分类：基于位置的粗分类（放宽标准，确保双侧检测）
        left_candidates = []
        right_candidates = []
        center_candidates = []  # 靠近中心的线条需要特别处理
        
        for line_data in valid_lines:
            features = line_data['features']
            center_x = features['center_x']
            
            # 放宽中心区域定义（从20%减少到15%），让更多线条直接归类
            center_margin = img_width * 0.075  # 参考src.py的稳定值
            
            if center_x < img_center - center_margin:
                left_candidates.append(line_data)
            elif center_x > img_center + center_margin:
                right_candidates.append(line_data)
            else:
                center_candidates.append(line_data)
        
        # 处理中心区域的线条 - 强制分配，不丢弃
        for line_data in center_candidates:
            features = line_data['features']
            
            # 基于角度和位置特征判断归属
            assignment = self.determine_line_assignment(features, img_width, left_candidates, right_candidates)
            
            if assignment == 'left':
                left_candidates.append(line_data)
            elif assignment == 'right':
                right_candidates.append(line_data)
            else:
                # 降级策略：即使无法确定，也尝试分配到较近的一侧
                center_x = features['center_x']
                if center_x < img_center:
                    left_candidates.append(line_data)  # 靠左就归左
                else:
                    right_candidates.append(line_data)  # 靠右就归右
        
        # 在每一侧内部去重和优化
        left_lines = self.optimize_side_lines(left_candidates, 'left', img_width)
        right_lines = self.optimize_side_lines(right_candidates, 'right', img_width)
        
        return left_lines, right_lines
    
    def determine_line_assignment(self, features, img_width, left_candidates, right_candidates):
        """确定中心区域线条的归属（参考src.py稳定版）"""
        center_x = features['center_x']
        angle = features['angle']
        img_center = img_width / 2
        
        # 基于角度判断（车道线通常有特定的倾斜方向）
        # 左侧线条通常角度为正（向右上倾斜），右侧线条角度为负（向右下倾斜）
        angle_weight = 0.0
        if angle > 10:  # 向右上倾斜，可能是左侧线
            angle_weight = -1.0
        elif angle < -10:  # 向右下倾斜，可能是右侧线
            angle_weight = 1.0
        
        # 基于位置权重
        position_weight = (center_x - img_center) / (img_width * 0.5)
        
        # 基于与已有线条的距离
        distance_weight = 0.0
        
        # 计算与左侧候选线条的平均距离
        if left_candidates:
            left_distances = [abs(center_x - candidate['features']['center_x']) for candidate in left_candidates]
            avg_left_distance = np.mean(left_distances)
        else:
            avg_left_distance = float('inf')
        
        # 计算与右侧候选线条的平均距离
        if right_candidates:
            right_distances = [abs(center_x - candidate['features']['center_x']) for candidate in right_candidates]
            avg_right_distance = np.mean(right_distances)
        else:
            avg_right_distance = float('inf')
        
        # 距离权重（更接近哪一侧）
        if avg_left_distance < avg_right_distance:
            distance_weight = -0.5
        elif avg_right_distance < avg_left_distance:
            distance_weight = 0.5
        
        # 综合判断（降低阈值，让更多线条能被归类）
        total_weight = position_weight + angle_weight + distance_weight
        
        # 降低判断阈值从 0.3 到 0.15，让更多线条能被明确归类（参考src.py）
        if total_weight < -0.15:  # 从 -0.3 降到 -0.15
            return 'left'
        elif total_weight > 0.15:  # 从 0.3 降到 0.15
            return 'right'
        else:
            return 'unknown'  # 仍无法确定的极少数情况
    def optimize_side_lines(self, candidates, side, img_width):
        """优化单侧的线条，去除重复和无效线条"""
        if not candidates:
            return []
        
        # 按质量排序（基于长度和位置合理性）
        scored_candidates = []
        for line_data in candidates:
            features = line_data['features']
            
            # 质量评分
            length_score = min(features['length'] / 200.0, 1.0)  # 长度分
            position_score = self.calculate_position_score(features, side, img_width)  # 位置合理性分
            
            total_score = length_score * 0.6 + position_score * 0.4
            
            scored_candidates.append({
                'line_data': line_data,
                'score': total_score
            })
        
        # 按分数排序
        scored_candidates.sort(key=lambda x: x['score'], reverse=True)
        
        # 去重：移除过于接近的线条
        final_lines = []
        for candidate in scored_candidates:
            line_data = candidate['line_data']
            features = line_data['features']
            
            # 检查是否与已选择的线条过于接近
            too_close = False
            for existing_line in final_lines:
                existing_features = existing_line['features']
                
                # 计算距离
                x_distance = abs(features['center_x'] - existing_features['center_x'])
                
                # 如果水平距离小于阈值，认为是重复线条
                if x_distance < 50:  # 可调整的阈值
                    too_close = True
                    break
            
            if not too_close:
                final_lines.append(line_data)
                
                # 限制每侧最多保留2条线
                if len(final_lines) >= 2:
                    break
        
        # 返回点集合列表
        return [line_data['points'] for line_data in final_lines]
    
    def calculate_position_score(self, features, side, img_width):
        """计算位置合理性分数"""
        center_x = features['center_x']
        img_center = img_width / 2
        
        if side == 'left':
            # 左侧线条应该在图像左半部分
            if center_x < img_center:
                return 1.0 - abs(center_x - img_center * 0.5) / (img_center * 0.5)
            else:
                return 0.2  # 惩罚右侧出现的"左侧"线条
        else:  # right
            # 右侧线条应该在图像右半部分
            if center_x > img_center:
                return 1.0 - abs(center_x - img_center * 1.5) / (img_center * 0.5)
            else:
                return 0.2  # 惩罚左侧出现的"右侧"线条
        
        return 0.5
    
    def validate_with_history(self, detected_lines, side, img_width):
        """使用历史信息验证和修正车道线检测结果"""
        if not detected_lines:
            return detected_lines
            
        history = self.left_lane_history if side == 'left' else self.right_lane_history
        
        # 如果没有历史数据，直接返回
        if not history:
            self.update_lane_history(detected_lines, side)
            return detected_lines
        
        # 计算每条检测线与历史的一致性
        validated_lines = []
        
        for line_points in detected_lines:
            if len(line_points) > 0:
                current_center = np.mean(line_points[:, 0])
                
                # 计算与历史位置的最小距离
                min_distance = float('inf')
                for hist_center in history:
                    distance = abs(current_center - hist_center)
                    min_distance = min(min_distance, distance)
                
                # 如果与历史一致，则保留
                if min_distance < self.lane_consistency_threshold:
                    validated_lines.append(line_points)
                else:
                    rospy.logwarn(f"车道线不一致检测 - {side} 侧: 当前位置 {current_center:.1f}, 历史最近 {min(history, key=lambda x: abs(x-current_center)):.1f}")
        
        # 更新历史
        self.update_lane_history(validated_lines, side)
        
        return validated_lines
    
    def update_lane_history(self, lines, side):
        """更新车道线历史位置"""
        history = self.left_lane_history if side == 'left' else self.right_lane_history
        
        if lines:
            # 计算当前检测的车道线中心位置
            centers = [np.mean(line[:, 0]) for line in lines if len(line) > 0]
            
            if centers:
                # 使用加权平均（更长的线条权重更大）
                weights = [len(line) for line in lines if len(line) > 0]
                avg_center = np.average(centers, weights=weights)
                
                # 添加到历史
                history.append(avg_center)
                
                # 限制历史长度
                if len(history) > self.max_lane_history:
                    history.pop(0)
        
        # 更新对应的历史列表
        if side == 'left':
            self.left_lane_history = history
        else:
            self.right_lane_history = history

    def detect_yellow_lanes(self, img):
        """基于Yellow Mask效果的车道线检测"""
        height, width = img.shape[:2]
        current_params = self.get_current_params()
        
        # 定义ROI区域
        roi_top = int(height * 0.3)
        roi_height = int(height * 0.4)
        roi = img[roi_top:roi_top+roi_height, 0:width]
        
        with self.display_lock:
            self.roi_img = roi.copy()
        
        # 提取黄色车道线
        hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
        
        yellow_mask1 = cv2.inRange(hsv, np.array([15, 80, 100]), np.array([30, 255, 255]))
        yellow_mask2 = cv2.inRange(hsv, np.array([30, 60, 100]), np.array([45, 255, 255]))
        yellow_mask3 = cv2.inRange(hsv, np.array([10, 50, 80]), np.array([35, 200, 200]))
        yellow_mask = cv2.bitwise_or(yellow_mask1, yellow_mask2)
        yellow_mask = cv2.bitwise_or(yellow_mask, yellow_mask3)
        
        # 形态学操作
        kernel_small = np.ones((2, 2), np.uint8)
        kernel_medium = np.ones((3, 3), np.uint8)
        kernel_large = np.ones((4, 4), np.uint8)
        
        yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_OPEN, kernel_small)
        yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_CLOSE, kernel_medium)
        yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_DILATE, kernel_large, iterations=1)
        
        with self.display_lock:
            self.mask_img = cv2.cvtColor(yellow_mask, cv2.COLOR_GRAY2BGR)
        
        # 车道线检测
        left_lines, right_lines = self.enhanced_lane_detection(yellow_mask)
        
        # 计算转向角度
        steering_angle = self.calculate_steering_angle_mask_based(
            left_lines, right_lines, len(left_lines) > 0, len(right_lines) > 0, width, height
        )
        
        # 创建结果图像
        result_img = self.create_debug_image(img, left_lines, right_lines, steering_angle, 
                                           roi_top, roi_height, current_params)
        
        return steering_angle, result_img

    def create_debug_image(self, img, left_lines, right_lines, steering_angle, 
                          roi_top, roi_height, current_params):
        """创建调试图像"""
        result_img = img.copy()
        height, width = img.shape[:2]
        
        # 绘制ROI区域
        cv2.rectangle(result_img, (0, roi_top), (width, roi_top + roi_height), (0, 255, 255), 2)
        
        # 绘制检测到的车道线
        detected_left = len(left_lines) > 0
        detected_right = len(right_lines) > 0
        
        if detected_left:
            for line_points in left_lines:
                for point in line_points:
                    cv2.circle(result_img, (int(point[0]), int(point[1] + roi_top)), 2, (0, 255, 0), -1)
                
                if len(line_points) > 5:
                    points_array = np.array(line_points, dtype=np.int32)
                    points_array[:, 1] += roi_top
                    cv2.polylines(result_img, [points_array], False, (0, 255, 0), 2)
                    
        if detected_right:
            for line_points in right_lines:
                for point in line_points:
                    cv2.circle(result_img, (int(point[0]), int(point[1] + roi_top)), 2, (0, 0, 255), -1)
                
                if len(line_points) > 5:
                    points_array = np.array(line_points, dtype=np.int32)
                    points_array[:, 1] += roi_top
                    cv2.polylines(result_img, [points_array], False, (0, 0, 255), 2)
        
        # 绘制中心线
        center_x = width / 2
        cv2.line(result_img, (int(center_x), height), 
                 (int(center_x), int(height*0.7)), (255, 0, 0), 2)
        
        # 添加文本信息
        self.add_debug_text(result_img, steering_angle, detected_left, detected_right, current_params)
        
        return result_img

    def add_debug_text(self, result_img, steering_angle, detected_left, detected_right, current_params):
        """添加调试文本信息"""
        # 显示巡线状态和模式
        if self.s_curve_mode:
            mode_text = "S-CURVE"
            status_color = (0, 255, 255)  # 黄色表示S弯模式
        elif self.reverse_mode:
            mode_text = "REVERSE"
            status_color = (255, 0, 255)  # 紫色表示倒车
        else:
            mode_text = "FORWARD"
            status_color = (0, 255, 0) if self.lane_following_enabled else (0, 0, 255)
            
        status_text = f"Lane Following: {'ON' if self.lane_following_enabled else 'OFF'} ({mode_text})"
        cv2.putText(result_img, status_text, (20, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, status_color, 2)
        
        # 显示动作执行状态
        action_text = f"Action: {'EXECUTING' if self.is_executing_action else 'IDLE'}"
        action_color = (0, 255, 255) if self.is_executing_action else (255, 255, 255)
        cv2.putText(result_img, action_text, (20, 70), cv2.FONT_HERSHEY_SIMPLEX, 1, action_color, 2)
        
        # 显示转向角度
        text = f"Steering: {steering_angle:.2f}"
        cv2.putText(result_img, text, (20, 110), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 2)
        
        # 显示检测状态
        if detected_left and detected_right:
            status = "Both Lanes"
            color = (0, 255, 0)
        elif detected_left:
            status = "Left Lane Only"
            color = (0, 200, 255)
        elif detected_right:
            status = "Right Lane Only"
            color = (0, 200, 255)
        else:
            status = "No Lanes"
            color = (0, 0, 255)
            
        cv2.putText(result_img, status, (20, 150), cv2.FONT_HERSHEY_SIMPLEX, 1, color, 2)
        
        # 显示参数信息
        offset_text = f"Lane Offset: {current_params['lane_offset']} px"
        cv2.putText(result_img, offset_text, (20, 190), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (200, 200, 200), 2)
        
        speed_text = f"Speed: {current_params['speed']:.2f} m/s"
        cv2.putText(result_img, speed_text, (20, 230), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (200, 200, 200), 2)
        
        # 显示车道线历史信息
        left_hist_text = f"L.Hist: {len(self.left_lane_history)} | R.Hist: {len(self.right_lane_history)}"
        cv2.putText(result_img, left_hist_text, (20, 270), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (200, 150, 200), 2)
        
        # 如果有历史数据，显示最近的位置
        if self.left_lane_history:
            last_left = self.left_lane_history[-1]
            cv2.putText(result_img, f"Last L: {last_left:.0f}px", (300, 270), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (100, 255, 100), 2)
        
        if self.right_lane_history:
            last_right = self.right_lane_history[-1]
            cv2.putText(result_img, f"Last R: {last_right:.0f}px", (450, 270), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (100, 100, 255), 2)

    def calculate_steering_angle_mask_based(self, left_lines, right_lines, detected_left, detected_right, img_width, img_height):
        """基于掩码的转向角度计算"""
        steering_angle = 0.0
        img_center = img_width / 2
        current_params = self.get_current_params()
        
        if detected_left and detected_right:
            # 双侧检测
            left_centers = [np.mean(line[:, 0]) for line in left_lines]
            right_centers = [np.mean(line[:, 0]) for line in right_lines]
            
            left_weights = [len(line) for line in left_lines]
            right_weights = [len(line) for line in right_lines]
            
            left_center = np.average(left_centers, weights=left_weights)
            right_center = np.average(right_centers, weights=right_weights)
            
            lane_center = (left_center + right_center) / 2
            steering_angle = (img_center - lane_center) / (img_width * 0.5)
            
        elif detected_left:
            # 仅左侧检测
            left_center = np.mean([np.mean(line[:, 0]) for line in left_lines])
            target_position = left_center + current_params['lane_offset']
            steering_angle = (img_center - target_position) / (img_width * 0.5)
            
        elif detected_right:
            # 仅右侧检测
            right_center = np.mean([np.mean(line[:, 0]) for line in right_lines])
            target_position = right_center - current_params['lane_offset']
            steering_angle = (img_center - target_position) / (img_width * 0.5)
            
        else:
            # 无检测
            self.no_lane_count += 1
            if self.lane_history:
                weights = np.linspace(0.1, 1.0, min(5, len(self.lane_history)))
                steering_angle = np.average(self.lane_history[-len(weights):], weights=weights)
            else:
                steering_angle = 0.0
        
        # 应用平滑和限制
        steering_angle = self.apply_steering_smoothing(steering_angle)
        
        # 不再使用最小转向幅度限制，S弯模式通过惯性参数自然实现
        
        return steering_angle

    def apply_steering_smoothing(self, raw_steering):
        """应用转向平滑"""
        current_params = self.get_current_params()
        smoothing_factor = current_params['steering_smoothing_factor']
        max_change = current_params['max_change']
        
        # 指数移动平均
        smoothed = smoothing_factor * raw_steering + (1 - smoothing_factor) * self.prev_steering
        
        # 变化率限制
        change = smoothed - self.prev_steering
        if abs(change) > max_change:
            smoothed = self.prev_steering + np.sign(change) * max_change
        
        self.prev_steering = smoothed
        
        # 更新历史
        self.steering_history.append(smoothed)
        if len(self.steering_history) > current_params['history_length']:
            self.steering_history.pop(0)
        
        if self.no_lane_count == 0:
            self.lane_history.append(smoothed)
            if len(self.lane_history) > current_params['history_length']:
                self.lane_history.pop(0)
        
        # 应用最大steering限制 
        max_steering = current_params.get('max_steering', 1.0)
        smoothed = max(-max_steering, min(max_steering, smoothed))
        
        return smoothed

    def publish_steering(self, steering):
        """发布转向控制指令"""
        if not self.lane_following_enabled:
            return
        
        current_params = self.get_current_params()
        akm_cmd = Akm()
        
        # 不使用动态调速，始终保持全速 
        speed_factor = 1.0  # 始终全速
        
        #  完全禁用检测质量调整 - 始终保持满速
        # if self.no_lane_count > 0:
        #     quality_factor = max(0.5, 1.0 - self.no_lane_count * 0.05)
        #     speed_factor *= quality_factor
        
        # 设置速度和转向
        base_speed = current_params['speed']
        steering_coeff = current_params['steering_coefficient']
        
        # === 添加死区：避免轮子对微小steering一直响应 ===
        STEERING_DEADZONE = 0.03  # steering死区阈值（绝对值）
        if abs(steering) < STEERING_DEADZONE:
            steering = 0.0
            rospy.logdebug(f"📍 Steering在死区内，归零: {abs(steering):.4f} < {STEERING_DEADZONE}")
        
        # 巡线模式下使用更保守的角速度限制，防止卡轮
        max_lane_angular = 1.2  # 巡线最大角速度（rad/s），比动作执行时小
        
        if self.reverse_mode:
            akm_cmd.linear = -base_speed * speed_factor  # 倒车为负速度
            akm_cmd.angular = -steering * steering_coeff  # 倒车时转向相反
            # 限制最大角速度，防止卡轮（倒车模式）
            akm_cmd.angular = max(-max_lane_angular, min(max_lane_angular, akm_cmd.angular))
            akm_cmd.diff = 0.0  # 禁用差速，防止卡轮
        else:
            akm_cmd.linear = base_speed * speed_factor
            akm_cmd.angular = steering * steering_coeff
            # 限制最大角速度，防止卡轮（前进模式）
            akm_cmd.angular = max(-max_lane_angular, min(max_lane_angular, akm_cmd.angular))
            akm_cmd.diff = 0.0  # 禁用差速，防止卡轮
        
        
        
        self.cmd_pub.publish(akm_cmd)
        self.last_steering_time = time.time()

    def toggle_reverse_mode(self):
        """切换倒车模式"""
        self.stop_vehicle()
        
        self.reverse_mode = not self.reverse_mode
        
        if self.reverse_mode:
            rospy.loginfo("切换到倒车模式，使用后置摄像头")
            rospy.loginfo(f"倒车参数: {self.reverse_params}")
            self.current_camera_topic = self.back_camera_topic
        else:
            rospy.loginfo("切换到前进模式，使用前置摄像头")
            rospy.loginfo(f"前进参数: {self.forward_params}")
            self.current_camera_topic = self.camera_topic
        
        # 重新订阅摄像头话题
        self.image_sub.unregister()
        self.image_sub = rospy.Subscriber(self.current_camera_topic, Image, self.image_callback)
        
        rospy.loginfo(f"已切换摄像头话题到: {self.current_camera_topic}")
        # 清除历史数据
        self.lane_history.clear()
        self.steering_history.clear()
        self.left_lane_history.clear()  # 清除左侧车道线历史
        self.right_lane_history.clear()  # 清除右侧车道线历史
        self.prev_steering = 0.0
        self.no_lane_count = 0

    def toggle_lane_following(self):
        """切换巡线状态"""
        self.lane_following_enabled = not self.lane_following_enabled
        
        if self.lane_following_enabled:
            rospy.loginfo("巡线功能已开启")
        else:
            rospy.loginfo("巡线功能已关闭")
            self.stop_vehicle()

    # === 交通标志动作执行方法 ===
    def execute_right_turn(self, turn_number=None):
        """执行90度右转"""
        if self.is_executing_action:
            rospy.logwarn("正在执行其他动作,跳过右转")
            return
        
        if turn_number is None:
            turn_number = self.action_counts['turn_right'] + 1
            
        if turn_number > 3:
            rospy.logwarn("右转次数超过限制(3次),跳过")
            return
            
        rospy.loginfo(f"开始执行第{turn_number}次右转动作")
        self.is_executing_action = True
        
        try:
            # 停车确保稳定状态
            self.stop_vehicle()
            rospy.sleep(0.8)
            # 检查里程计数据是否可用
            if self.current_pose is None:
                rospy.logwarn("里程计数据不可用,使用时间控制右转")
                if turn_number == 1:
                    self.publish_cmd_vel(-0.2,0)
                    rospy.sleep(1.2)
                    # 第1次左转：增大角速度
                    self.publish_cmd_vel(0.1, 0.0)
                    rospy.sleep(0.5)
                    self.publish_cmd_vel(0.32, -1.60)  # 适中的角速度 0.32->0.28
                    rospy.sleep(1.8)
                
                elif turn_number == 2:
                    self.publish_cmd_vel(-0.2,0)
                    rospy.sleep(0.8)
                    # 第2次左转：增大角速度，避免压左线
                    self.publish_cmd_vel(0.1, 0.0)
                    rospy.sleep(0.5)
                    self.publish_cmd_vel(0.2, -1.60)  # 提高线速度和角速度
                    rospy.sleep(2.1)
                    
                else:
                    # 第3次左转：增大角速度
                    self.publish_cmd_vel(-0.2,0)
                    rospy.sleep(1.3)
                    self.publish_cmd_vel(0.1, 0.0)
                    rospy.sleep(0.3)
                    self.publish_cmd_vel(0.32, -1.65)  # 进一步提高角速度
                    rospy.sleep(1.6)

                self.stop_vehicle()
                rospy.loginfo("时间控制右转完成")
            else:
                # 使用里程计精确控制
                initial_yaw = self.current_yaw
                target_yaw = initial_yaw + (math.pi/2)
                target_yaw = math.atan2(math.sin(target_yaw), math.cos(target_yaw))
                
                rospy.loginfo(f"初始角度: {math.degrees(initial_yaw):.1f}度")
                rospy.loginfo(f"目标角度: {math.degrees(target_yaw):.1f}度")
                
                # 根据左转次数设置角速度增益
                if turn_number == 1:
                    angular_multiplier = 1.15  # 第1次左转：增强15%
                elif turn_number == 2:
                    angular_multiplier = 1.20  # 第2次左转：增强20%
                else:
                    angular_multiplier = 1.10  # 第3次左转：增强10%
                
                # 执行左转
                rate = rospy.Rate(20)
                start_time = rospy.get_time()
                timeout = 6.0
                last_yaw_diff = float('inf')
                stuck_count = 0
                
                while not rospy.is_shutdown():
                    current_time = rospy.get_time()
                    elapsed_time = current_time - start_time
                    
                    if elapsed_time > timeout:
                        rospy.logwarn(f"左转动作超时({timeout}秒),强制停止")
                        break
                    
                    if self.current_pose is None:
                        rospy.logwarn("里程计数据丢失,停止左转")
                        break
                        
                    yaw_diff = target_yaw - self.current_yaw
                    yaw_diff = math.atan2(math.sin(yaw_diff), math.cos(yaw_diff))
                    
                    rospy.loginfo(f"当前角度: {math.degrees(self.current_yaw):.1f}度, "
                                 f"角度差: {math.degrees(yaw_diff):.1f}度")
                    
                    if abs(yaw_diff) < 0.15:
                        rospy.loginfo(f"左转完成! 最终角度: {math.degrees(self.current_yaw):.1f}度")
                        break
                    
                    if abs(abs(yaw_diff) - abs(last_yaw_diff)) < 0.02:
                        stuck_count += 1
                        if stuck_count > 20:
                            rospy.logwarn("左转可能卡住,强制停止")
                            break
                    else:
                        stuck_count = 0
                    
                    last_yaw_diff = yaw_diff
                    
                    # 动态调整速度 - 根据左转次数增强角速度
                    if abs(yaw_diff) > 0.5:
                        angular_vel = 0.95 * angular_multiplier  # 提高基础角速度 0.85->0.95
                        linear_vel = 0.32  # 提高线速度
                    elif abs(yaw_diff) > 0.2:
                        angular_vel = 0.72 * angular_multiplier  # 提高角速度 0.65->0.72
                        linear_vel = 0.28
                    else:
                        angular_vel = 0.42 * angular_multiplier  # 提高角速度 0.35->0.42
                        linear_vel = 0.22
                    
                    # 确保左转方向(yaw_diff为正时向左转,angular_vel为正)
                    if yaw_diff < 0:
                        angular_vel = -angular_vel
                    
                    self.publish_cmd_vel(linear_vel, angular_vel)
                    rate.sleep()
            
            self.stop_vehicle()
            rospy.sleep(0.3)
            
            # 左转后姿态调整：微向右偏，防止压左线
            rospy.loginfo(f"第{turn_number}次左转后姿态调整，防止压左线")
            if turn_number == 1:
                # 第1次左转后：略向右微调
                self.publish_cmd_vel(0.25, 0.15)  # 小幅右转
                rospy.sleep(0.35)
            elif turn_number == 2:
                # 第2次左转后：略向右微调
                self.publish_cmd_vel(0.25, 0.18)  # 稍大幅度右转
                rospy.sleep(0.4)
            else:
                # 第3次左转后：略向右微调
                self.publish_cmd_vel(0.25, 0.12)  # 轻微右转
                rospy.sleep(0.3)
            
            self.stop_vehicle()
            rospy.sleep(0.2)
            
            rospy.loginfo(f"第{turn_number}次左转动作完成")
            self.left_lane_history.clear()
            self.right_lane_history.clear()
            
        except Exception as e:
            rospy.logerr(f"第{turn_number}次左转执行错误: {e}")
            traceback.print_exc()
        finally:
            self.stop_vehicle()
            self.action_counts['turn_right'] = turn_number
            self.is_executing_action = False
            
            # 第三次左转后，记录完成时间，用于延迟激活S弯模式
            if turn_number == 1:
                self.first_turn_completed_time = time.time()
                rospy.loginfo("=" * 50)
                rospy.loginfo(" 第1次左转完成，将在1.5s后激活S弯模式！")
                rospy.loginfo("=" * 50)
            
            rospy.sleep(1.5)
            self.lane_following_enabled = True
    
    # === 新增动作执行方法 ===
    def execute_left_turn(self):
        """执行左转动作 - 使用里程计提高精度"""
        if self.is_executing_action or self.left_turn_executed:
            rospy.logwarn("正在执行其他动作或左转已执行过")
            return
            
        self.is_executing_action = True
        self.left_turn_executed = True
        rospy.loginfo("开始执行右转动作（使用里程计）")
        
        try:
            # # 停车
            # self.stop_vehicle()
            # rospy.sleep(0.5)
            
            # 记录初始角度
            # if self.current_pose is None:
            # rospy.logwarn("里程计数据不可用，使用时间控制")
                # 备用时间控制方案
            self.publish_cmd_vel(0.1, 0)
            rospy.sleep(1.0)
            self.publish_cmd_vel(0.22, 0.9)  # 负值为右转
            rospy.sleep(2.5)
            # else:
            #     initial_yaw = self.current_yaw
            #     target_yaw = initial_yaw - (math.pi/2-0.05)  # 右转90度
                
            #     # 角度归一化到[-pi, pi]
            #     target_yaw = math.atan2(math.sin(target_yaw), math.cos(target_yaw))
                
            #     rospy.loginfo(f"初始角度: {math.degrees(initial_yaw):.1f}度")
            #     rospy.loginfo(f"目标角度: {math.degrees(target_yaw):.1f}度")
                
            #     # 执行右转
            #     #turn_timeout = 10.0  # 最大转向时间
            #     turn_timeout = 4 # 最大转向时间
            #     start_time = rospy.get_time()
                
            #     while not rospy.is_shutdown():
            #         current_time = rospy.get_time()
            #         if current_time - start_time > turn_timeout:
            #             rospy.logwarn("右转超时，停止转向")
            #             break
                    
            #         if self.current_pose is None:
            #             rospy.logwarn("里程计数据丢失")
            #             break
                    
            #         # 计算角度差
            #         angle_diff = target_yaw - self.current_yaw
            #         # 角度差归一化到[-pi, pi]
            #         angle_diff = math.atan2(math.sin(angle_diff), math.cos(angle_diff))
                    
            #         # 检查是否到达目标角度
            #         if abs(angle_diff) < 0.05:  # 约3度的误差范围
            #             rospy.loginfo(f"右转完成，当前角度: {math.degrees(self.current_yaw):.1f}度")
            #             break
                    
            #         # 根据角度差调整转向速度
            #         #angular_speed = max(0.2, min(0.5, abs(angle_diff) * 0.8))
            #         angular_speed = max(0.5, min(0.60, abs(angle_diff))) # 0.5 0.5
            #         if angle_diff > 0:
            #             angular_speed = angular_speed  # 左转
            #         else:
            #             angular_speed = -angular_speed  # 右转
                    
            #         #self.publish_cmd_vel(0.1, angular_speed)
            #         self.publish_cmd_vel(0.25, angular_speed) # 0.25
            #         rospy.sleep(0.1)
            
            # 停车
            self.stop_vehicle()
            rospy.sleep(0.3)
            rospy.loginfo("左转动作执行完成")
            self.left_lane_history.clear()
            self.right_lane_history.clear()
            
            # 右转后恢复正常偏移，避免往右驶出
            self.forward_params['lane_offset'] = 270
            
        except Exception as e:
            rospy.logerr(f"左转动作执行错误: {e}")
        finally:
            self.is_executing_action = False
            self.slow_mode = True
            # 更新动作计数器（统一在finally中）
            self.action_counts['left_turn'] = 1

    def execute_move_left_and_start_lane_following(self):
        """执行靠左行驶并开启巡线"""
        if self.is_executing_action or self.yaxian_executed:
            rospy.logwarn("正在执行其他动作或靠左行驶已执行过")
            return
            
        self.is_executing_action = True
        self.yaxian_executed = True
        rospy.loginfo("开始执行靠左行驶动作")
        
        try:
            # 确保车辆完全停止
            self.stop_vehicle()
            rospy.sleep(1.0)

            self.publish_cmd_vel(-0.1, 0)
            rospy.sleep(1)
            # self.publish_cmd_vel(0.1,0)
            # rospy.sleep(1)
            # self.publish_cmd_vel(0.1,0.45)
            # rospy.sleep(1.5)
            # self.publish_cmd_vel(0.1,-0.55)
            # rospy.sleep(1.35)
        
            rospy.loginfo("前进到左侧车道")
            self.publish_cmd_vel(0.2, 0.0)
            rospy.sleep(3)

            self.stop_vehicle()
            rospy.loginfo("靠左侧行驶完成")
            self.left_lane_history.clear()
            self.right_lane_history.clear()
            
            #  关键修改：强制关闭S弯模式，恢复正常巡线
            if self.s_curve_mode:
                rospy.loginfo("🔄 靠左行驶完成，关闭S弯模式，恢复正常巡线")
                self.s_curve_mode = False
                self.s_curve_activation_time = None
                self.first_turn_completed_time = None  # 重置第一次左转时间，防止再次激活
            
            # 单边桥慢速
            self.slow_mode = False
            # 开启巡线
            self.lane_following_enabled = True
            rospy.loginfo("靠左行驶动作执行完成，使用正常巡线参数")
            
        except Exception as e:
            rospy.logerr(f"靠左行驶动作执行错误: {e}")
        finally:
            self.is_executing_action = False
            self.lane_following_enabled = True

    def execute_stop_and_continue(self):
        """执行停车2秒后继续巡线"""
        if self.is_executing_action:
            rospy.logwarn("正在执行其他动作")
            return
            
        self.is_executing_action = True
        rospy.loginfo("开始执行停车动作")
        
        try:
            # 停车2秒
            self.stop_vehicle()
            rospy.sleep(2.0)
            
            #  关键修改：确保使用正常巡线参数
            if self.s_curve_mode:
                rospy.loginfo("🔄 停车完成，关闭S弯模式，恢复正常巡线")
                self.s_curve_mode = False
                self.s_curve_activation_time = None
            
            # 继续巡线
            self.lane_following_enabled = True
            rospy.loginfo("停车动作执行完成，使用正常巡线参数继续")
            
        except Exception as e:
            rospy.logerr(f"停车动作执行错误: {e}")
        finally:
            self.is_executing_action = False
            self.lane_following_enabled = True

    def execute_reverse_lane_following(self):
        """执行倒车巡线"""
        if self.is_executing_action or self.daoche_executed:
            rospy.logwarn("正在执行其他动作或倒车巡线已执行过")
            return
            
        self.is_executing_action = True
        self.daoche_executed = True
        rospy.loginfo("开始执行倒车巡线动作")
        
        try:
            self.lane_following_enabled = False
            self.stop_vehicle()
            rospy.sleep(0.5)
            
            #  新增：关闭S弯模式
            if self.s_curve_mode:
                rospy.loginfo("检测到S弯模式，自动关闭以使用倒车参数")
                self.s_curve_mode = False
                self.s_curve_activation_time = None
            
            # 清除历史数据
            self.lane_history.clear()
            self.steering_history.clear()
            self.left_lane_history.clear()
            self.right_lane_history.clear()
            self.prev_steering = 0.0
            
            # 切换到后摄像头
            if not self.reverse_mode:
                self.toggle_reverse_mode()
            
            # 开启倒车巡线
            self.lane_following_enabled = True
            rospy.loginfo("倒车巡线动作执行完成")
            
        except Exception as e:
            rospy.logerr(f"倒车巡线动作执行错误: {e}")
        finally:
            self.is_executing_action = False
            self.action_counts['daoche'] = 1
    def execute_left_rear_reverse(self):
        """执行左后方倒车动作"""
        if self.is_executing_action or self.fanzhuan_executed:
            rospy.logwarn("正在执行其他动作或左后方倒车已执行过")
            return
            
        self.is_executing_action = True
        self.fanzhuan_executed = True
        rospy.loginfo("开始执行左后方倒车动作")
        
        try:
            # 停止倒车巡线
            self.lane_following_enabled = False
            self.stop_vehicle()
            rospy.sleep(0.5)
            self.publish_cmd_vel(0.2, 0)
            rospy.sleep(1.0)
            
            # 左后方倒车
            self.publish_cmd_vel(-0.35, -1)
            rospy.sleep(2.4)
            self.publish_cmd_vel(0.1,0.2)
            rospy.sleep(0.5)
            
            # 停车
            self.stop_vehicle()
            rospy.sleep(0.5)
            
            # 切换到前摄像头
            if self.reverse_mode:
                self.toggle_reverse_mode()
            
            #  翻转完成后，确保使用正常巡线参数
            if self.s_curve_mode:
                rospy.loginfo("🔄 左后方倒车完成，关闭S弯模式，恢复正常巡线")
                self.s_curve_mode = False
                self.s_curve_activation_time = None
                self.first_turn_completed_time = None  # 防止再次激活S弯
            
            # 开启前方巡线
            self.lane_following_enabled = True
            rospy.loginfo("左后方倒车动作执行完成，使用正常巡线参数")
            
        except Exception as e:
            rospy.logerr(f"左后方倒车动作执行错误: {e}")
        finally:
            self.is_executing_action = False

    def execute_final_stop(self):
        """执行最终停止动作"""
        if self.is_executing_action or self.final_executed:
            rospy.logwarn("正在执行其他动作或最终停止已执行过")
            return
            
        self.is_executing_action = True
        self.final_executed = True
        rospy.loginfo("开始执行最终停止动作")
        
        try:
            #  第1步：先关闭巡线（优先级最高）
            self.lane_following_enabled = False
            rospy.loginfo("已关闭巡线功能")
            
            #  第2步：关闭所有特殊模式
            if self.s_curve_mode:
                self.s_curve_mode = False
                self.s_curve_activation_time = None
                rospy.loginfo("已关闭S弯模式")
            
            if self.reverse_mode:
                # 如果在倒车模式，切换回前进模式
                self.reverse_mode = False
                rospy.loginfo("已关闭倒车模式")
            
            self.slow_mode = False
            
            #  第3步：多次发送停止指令，确保生效
            for i in range(5):
                self.stop_vehicle()
                rospy.sleep(0.1)
            
            rospy.loginfo("=" * 50)
            rospy.loginfo(" 最终停止动作执行完成，车辆已完全停止")
            rospy.loginfo("=" * 50)
            
            #  第4步：持续发送停止指令，防止被其他线程覆盖
            rospy.sleep(1.0)
            self.stop_vehicle()
            
        except Exception as e:
            rospy.logerr(f"最终停止动作执行错误: {e}")
        finally:
            #  保持动作执行状态为True，防止其他线程干扰
            #self.is_executing_action = False  # 注释掉这行
            
            #  确保巡线关闭
            self.lane_following_enabled = False
            
            #  再次发送停止指令
            self.stop_vehicle()
            
            rospy.loginfo("车辆已锁定在停止状态")
    
    def execute_reverse_parking(self):
        """执行左侧倒车入库动作（最多执行一次）"""
        if self.is_executing_action:
            rospy.logwarn("正在执行其他动作，跳过倒车入库")
            return
            
        if self.reverse_parking_executed:
            rospy.logwarn("倒车入库动作已执行过，不再重复执行")
            return
            
        rospy.loginfo("开始执行左侧倒车入库动作")
        self.is_executing_action = True
        
        try:
            # 左侧倒车入库序列
            rospy.loginfo("前进到倒车位置")
            self.publish_cmd_vel(self.parking_linear_speed, 0.0)
            rospy.sleep(1)

            self.publish_cmd_vel(-0.2, 0.0)
            rospy.sleep(0.5)
            
            rospy.loginfo("左转倒车进入车位")
            self.publish_cmd_vel(-0.38, -1.6)
            rospy.sleep(1.5)
            

            rospy.loginfo("回正继续倒车")
            self.publish_cmd_vel(-0.2, 0.0)
            rospy.sleep(1.3)
            
            rospy.loginfo("前进")
            self.publish_cmd_vel(self.parking_linear_speed, 0.0)
            rospy.sleep(2.1)

            rospy.loginfo("左转前进移出车位")
            self.publish_cmd_vel(0.2, 1.3)
            rospy.sleep(2.3)
            
            rospy.loginfo("前进")
            self.publish_cmd_vel(0.2, 0.0)
            rospy.sleep(2.0)
            

            rospy.loginfo("左转倒车进入车位")
            self.publish_cmd_vel(-0.25, 1.2)
            rospy.sleep(2.2)
            
            rospy.loginfo("回正继续倒车")
            self.publish_cmd_vel(-0.2, 0.0)
            rospy.sleep(1.0)

            rospy.loginfo("前进")
            self.publish_cmd_vel(self.parking_linear_speed, 0.0)
            rospy.sleep(2)

            rospy.loginfo("右转前进移出车位")
            self.publish_cmd_vel(0.25, -1)
            rospy.sleep(2.0)
            
            self.stop_vehicle()
            
            # 标记倒车入库已执行
            self.reverse_parking_executed = True
            rospy.loginfo("左侧倒车入库完成（已标记为已执行）")
            
        except Exception as e:
            rospy.logerr(f"倒车入库执行错误: {e}")
        finally:
            self.is_executing_action = False
            self.prev_steering = 0.0
            self.lane_history.clear()
            self.steering_history.clear()
            self.left_lane_history.clear()
            self.right_lane_history.clear()
            self.no_lane_count = 0

            # ✅ 关闭所有特殊模式
            self.s_curve_mode = False
            self.s_curve_activation_time = None
            self.first_turn_completed_time = None

            #  再开启巡线
            self.lane_following_enabled = True
            rospy.loginfo("倒车入库完成，已清除历史状态，恢复正常巡线")
            rospy.loginfo("倒车入库完成，使用正常巡线参数")
            # 更新动作计数器
            self.action_counts['reverse'] = 1
    def velocity(self, x, z, d):
        cmd = Akm()
        cmd.linear = x
        cmd.angular = z
        cmd.diff = d  # 默认差速为0
        self.cmd_pub.publish(cmd)

    def execute_parallel_parking(self):
        """执行左侧方位停车动作（最多执行一次）"""
        if self.is_executing_action:
            rospy.logwarn("正在执行其他动作，跳过侧方停车")
            return
            
        if self.parallel_parking_executed:
            rospy.logwarn("侧方停车动作已执行过，不再重复执行")
            return
            
        rospy.loginfo("开始执行左侧方位停车动作")
        self.is_executing_action = True
        
        try:
            # self.publish_cmd_vel(-0.2, 0)
            # rospy.sleep(1.0)
            # # 侧方停车序列
            # rospy.loginfo("前进到停车位置")
            # self.publish_cmd_vel(self.parking_linear_speed, 0.0)
            # rospy.sleep(0.8)

            # rospy.loginfo("左转倒车进入车位")
            # self.publish_cmd_vel(-0.3, 0.7) #0.5->0.7
            # rospy.sleep(1.8)#1.7

            # rospy.loginfo("回正继续倒车")
            # self.publish_cmd_vel(-0.2, 0.0)
            # rospy.sleep(0.5) #0.5

            # rospy.loginfo("右转调整车身")
            # self.velocity(-0.1, -0.85, 0.07) #0.05
            # rospy.sleep(4)

            # rospy.loginfo("右转调整车身")
            # self.velocity(0.1, -0.95, 0.08) #-0.7
            # rospy.sleep(3.2)


            # rospy.loginfo("左转回到主道")
            # self.publish_cmd_vel(0.25, 0.65)#0.75
            # rospy.sleep(1.0)
            # rospy.loginfo("右转调整车身")
            # self.publish_cmd_vel(-0.2, -0.4)
            # rospy.sleep(1.5)
            
            # rospy.loginfo("左转前进")
            # self.publish_cmd_vel(self.parking_linear_speed*0.6, self.parking_angular_speed)
            # rospy.sleep(1)
            
            # rospy.loginfo("左转倒车进入车位")
            # self.publish_cmd_vel(-0.2, self.parking_angular_speed*0.6)
            # rospy.sleep(0.5)
            
            # self.stop_vehicle()
            # rospy.loginfo("侧方停车完成，开始出库动作")
            
            # # 侧方停车出库动作序列
            # rospy.loginfo("前进微调")
            # self.publish_cmd_vel(self.parking_linear_speed, 0.0)
            # rospy.sleep(1.2)

            # rospy.loginfo("左转倒车进入车位")
            # self.publish_cmd_vel(-0.2, 0.4)
            # rospy.sleep(1.5)
            
            # # 1. 右转前进开始出库
            # rospy.loginfo("右转前进出库")
            # self.publish_cmd_vel(0.3, -0.5)
            # rospy.sleep(0.8)
            
            # # 2. 左转调整车身方向
            # rospy.loginfo("左转调整方向")
            # self.publish_cmd_vel(0.25, 0.4)
            # rospy.sleep(1.8)
            
            # # 3. 回正方向盘前进到道路中央
            # rospy.loginfo("回正前进到道路中央")
            # self.publish_cmd_vel(self.parking_linear_speed, 0.0)
            # rospy.sleep(1.5)
            
        # 1. 右转倒车入库
            self.publish_cmd_vel(0.2, 0)
            rospy.sleep(0.5)

            self.publish_cmd_vel(-0.3, -0.60)
            rospy.sleep(2.5)

        # 2. 右打倒车
            self.publish_cmd_vel(-0.15, 0.5)
            rospy.sleep(0.9)

        # 4. 左打调整车头
            self.publish_cmd_vel(0.08, -0.75)
            rospy.sleep(1.3)
            self.publish_cmd_vel(-0.1, 0)
            rospy.sleep(1.8)
            self.publish_cmd_vel(0.08, -0.75)
            rospy.sleep(1.3)

        # 3. 回正倒车
            self.publish_cmd_vel(-0.15, 0.75)
            rospy.sleep(1.0)
            self.publish_cmd_vel(0, 0)
            rospy.sleep(0.5)
            #  # self.publish_cmd_vel(-0.15, 0)
            #  #rospy.sleep(0.5)
        # 5. 右打回正车身
            # self.publish_cmd_vel(0.1, -0.4)
            # rospy.sleep(1)
            # self.publish_cmd_vel(-0.1, 0)
            # rospy.sleep(1.4)
            self.publish_cmd_vel(0.08, 0.75)
            rospy.sleep(1.3)
            self.publish_cmd_vel(0.17, 1.2)
            rospy.sleep(0.9)
            self.publish_cmd_vel(0.15, 0)
            rospy.sleep(0.5)
            self.publish_cmd_vel(0.2, -0.8)
            rospy.sleep(1.8)
        # 6. 直线微调停车
            # self.publish_cmd_vel(0.15, 0)
            # rospy.sleep(0.8)

            self.stop_vehicle()
            rospy.loginfo("侧方停车出库动作完成")
            self.left_lane_history.clear()
            self.right_lane_history.clear()
            rospy.loginfo("已清除车道线历史数据")
            
            # 标记侧方停车已执行
            self.parallel_parking_executed = True
            rospy.loginfo("侧方停车和出库完成（已标记为已执行）")
            
        except Exception as e:
            rospy.logerr(f"侧方停车执行错误: {e}")
        finally:
            self.is_executing_action = False
            
            #  关键修改：侧方停车完成后，确保使用正常巡线参数
            if self.s_curve_mode:
                rospy.loginfo("🔄 侧方停车完成，关闭S弯模式，恢复正常巡线")
                self.s_curve_mode = False
                self.s_curve_activation_time = None
            
            # 动作完成后重新启用车道跟随
            self.lane_following_enabled = True
            rospy.loginfo("侧方停车完成，使用正常巡线参数")
            # 更新动作计数器
            self.action_counts['parallel_parking'] = 1

    def reset_parking_status(self):
        """重置停车状态（可选功能）"""
        if self.allow_action_reset:
            self.reverse_parking_executed = False
            self.parallel_parking_executed = False
            self.right_turn_executed = False
            self.yaxian_executed = False
            self.daoche_executed = False
            self.fanzhuan_executed = False
            self.final_executed = False
            self.action_counts = {
                'turn_left': 0,
                'reverse': 0,
                'parallel_parking': 0,
                'right_turn': 0,
                'yaxian': 0,
                'stop': 0,
                'daoche': 0,
                'fanzhuan': 0,
                'final': 0
            }
            rospy.loginfo("停车状态已重置，可重新执行停车动作")
        else:
            rospy.logwarn("停车状态重置功能未启用")

    # === 显示更新方法 ===
    def update_display(self):
        """更新显示和处理键盘输入"""
        if not self.debug:
            return
            
        with self.display_lock:
            if self.result_img is not None:
                cv2.imshow('Lane Detection', self.result_img)
            
            if self.roi_img is not None:
                cv2.imshow('ROI', self.roi_img)
            
            if self.mask_img is not None:
                cv2.imshow('Yellow Mask', self.mask_img)
        
        key = cv2.waitKey(1) & 0xFF
        if key == ord('h'):
            self.print_help()
        elif key == ord('s'):
            self.print_current_status()
        elif key == ord('q'):
            rospy.signal_shutdown("User requested shutdown")
        elif key == ord(' '):
            self.toggle_lane_following()
        elif key == ord('c'):
            self.toggle_reverse_mode()
        elif key == ord('o'):
            current_params = self.get_current_params()
            current_params['lane_offset'] += 10
            rospy.loginfo(f"增加车道偏移到 {current_params['lane_offset']}px")
        elif key == ord('p'):
            current_params = self.get_current_params()
            current_params['lane_offset'] = max(50, current_params['lane_offset'] - 10)
            rospy.loginfo(f"减少车道偏移到 {current_params['lane_offset']}px")
        elif key == ord('1'):
            if not self.is_executing_action and self.action_counts['turn_left'] < self.max_left_turns:
                threading.Thread(target=self.execute_left_turn).start()
        elif key == ord('2'):
            if not self.is_executing_action:
                threading.Thread(target=self.execute_reverse_parking).start()
        elif key == ord('3'):
            if not self.is_executing_action:
                threading.Thread(target=self.execute_parallel_parking).start()
        elif key == ord('4'):
            if not self.is_executing_action and not self.right_turn_executed:
                threading.Thread(target=self.execute_right_turn).start()
        elif key == ord('5'):
            if not self.is_executing_action and not self.yaxian_executed:
                threading.Thread(target=self.execute_move_left_and_start_lane_following).start()
        elif key == ord('6'):
            if not self.is_executing_action:
                threading.Thread(target=self.execute_stop_and_continue).start()
        elif key == ord('7'):
            if not self.is_executing_action and not self.daoche_executed:
                threading.Thread(target=self.execute_reverse_lane_following).start()
        elif key == ord('8'):
            if not self.is_executing_action and not self.fanzhuan_executed:
                threading.Thread(target=self.execute_left_rear_reverse).start()
        elif key == ord('9'):
            if not self.is_executing_action and not self.final_executed:
                threading.Thread(target=self.execute_final_stop).start()
        elif key == ord('r'):
            self.reset_parking_status()

    def run(self):
        """主运行循环"""
        rate = rospy.Rate(20)
        safety_timer = time.time()
        
        try:
            while not rospy.is_shutdown():
                self.update_display()
                
                #  修改：S弯模式仅在第一次右转后激活，且不被其他动作影响
                if (self.first_turn_completed_time is not None and 
                    not self.s_curve_mode and 
                    not self.yaxian_executed):  # ← 添加此条件，yaxian执行后不激活
                    elapsed = time.time() - self.first_turn_completed_time
                    if elapsed >= 1.0:
                        rospy.loginfo("=" * 60)
                        rospy.loginfo("🌊 激活S弯模式！使用惯性参数，防止过早切换方向")
                        rospy.loginfo("=" * 60)
                        self.s_curve_mode = True
                        self.s_curve_activation_time = time.time()
                        # 清除历史数据
                        self.lane_history.clear()
                        self.steering_history.clear()
                        self.left_lane_history.clear()
                        self.right_lane_history.clear()
                        self.prev_steering = 0.0
                
                # S弯模式自动停用（激活后指定时间）
                if self.s_curve_mode and self.s_curve_activation_time is not None:
                    elapsed = time.time() - self.s_curve_activation_time
                    if elapsed >= self.s_curve_duration:
                        rospy.loginfo("=" * 60)
                        rospy.loginfo(f"✅ S弯模式结束（已运行{elapsed:.1f}秒），恢复普通巡线")
                        rospy.loginfo("=" * 60)
                        self.s_curve_mode = False
                        self.s_curve_activation_time = None
                        # 清除历史数据
                        self.lane_history.clear()
                        self.steering_history.clear()
                        self.left_lane_history.clear()
                        self.right_lane_history.clear()
                        self.prev_steering = 0.0
                
                # 安全机制
                if (self.lane_following_enabled and not self.is_executing_action and
                    time.time() - self.last_steering_time > 1.0):
                    rospy.logwarn("No steering update for 1 second. Stopping vehicle.")
                    self.stop_vehicle()
                    self.last_steering_time = time.time()
                
                # 定期状态报告
                if time.time() - safety_timer > 10.0:
                    lane_status = "开启" if self.lane_following_enabled else "关闭"
                    mode = "倒车" if self.reverse_mode else "前进"
                    action_status = "执行中" if self.is_executing_action else "空闲"
                    rospy.loginfo(f"集成控制器运行中 - 车道跟随: {lane_status}, 模式: {mode}, 动作状态: {action_status}")
                    safety_timer = time.time()
                
                rate.sleep()
                
        except KeyboardInterrupt:
            rospy.loginfo("KeyboardInterrupt received")
        except Exception as e:
            rospy.logerr(f"Unexpected error: {e}")
            traceback.print_exc()
        finally:
            self.cleanup()
            if not rospy.is_shutdown():
                rospy.signal_shutdown("Normal termination")

def main():
    """主函数"""
    controller = None
    try:
        controller = IntegratedVehicleController()
        controller.run()
    except rospy.ROSInterruptException:
        rospy.loginfo("集成控制器节点被中断")
    except KeyboardInterrupt:
        rospy.loginfo("用户中断程序")
    except Exception as e:
        rospy.logerr(f"集成控制器启动失败: {e}")
        traceback.print_exc()
    finally:
        if controller:
            controller.cleanup()
        cv2.destroyAllWindows()
        rospy.loginfo("程序已退出")

if __name__ == '__main__':
    main()
